home *** CD-ROM | disk | FTP | other *** search
/ Aminet 51 / Aminet 51 (2002)(GTI - Schatztruhe)[!][Oct 2002].iso / Aminet / dev / c / TinyGL.lha / tinygl / src / zmath.c < prev    next >
Encoding:
C/C++ Source or Header  |  2000-10-15  |  5.3 KB  |  276 lines
  1. /* Some simple mathematical functions. Don't look for some logic in
  2.    the function names :-) */
  3.  
  4. #include <stdlib.h>
  5. #include <string.h>
  6. #include <math.h>
  7. #include "zmath.h"
  8.  
  9.  
  10. /* ******* Gestion des matrices 4x4 ****** */
  11.  
  12. void gl_M4_Id(M4 *a)
  13. {
  14.     int i,j;
  15.     for(i=0;i<4;i++)
  16.     for(j=0;j<4;j++) 
  17.     if (i==j) a->m[i][j]=1.0; else a->m[i][j]=0.0;
  18. }
  19.  
  20. int gl_M4_IsId(M4 *a)
  21. {
  22.     int i,j;
  23.     for(i=0;i<4;i++)
  24.     for(j=0;j<4;j++) {
  25.       if (i==j) { 
  26.         if (a->m[i][j] != 1.0) return 0;
  27.       } else if (a->m[i][j] != 0.0) return 0;
  28.     }
  29.   return 1;
  30. }
  31.  
  32. void gl_M4_Mul(M4 *c,M4 *a,M4 *b)
  33. {
  34.   int i,j,k;
  35.   float s;
  36.   for(i=0;i<4;i++)
  37.     for(j=0;j<4;j++) {
  38.       s=0.0;
  39.       for(k=0;k<4;k++) s+=a->m[i][k]*b->m[k][j];
  40.       c->m[i][j]=s;
  41.     }
  42. }
  43.  
  44. /* c=c*a */
  45. void gl_M4_MulLeft(M4 *c,M4 *b)
  46. {
  47.   int i,j,k;
  48.   float s;
  49.   M4 a;
  50.  
  51.   /*memcpy(&a, c, 16*sizeof(float));
  52.   */
  53.   a=*c;
  54.  
  55.   for(i=0;i<4;i++)
  56.     for(j=0;j<4;j++) {
  57.       s=0.0;
  58.       for(k=0;k<4;k++) s+=a.m[i][k]*b->m[k][j];
  59.       c->m[i][j]=s;
  60.     }
  61. }
  62.  
  63. void gl_M4_Move(M4 *a,M4 *b)
  64. {
  65.     memcpy(a,b,sizeof(M4));
  66. }
  67.  
  68. void gl_MoveV3(V3 *a,V3 *b)
  69. {
  70.     memcpy(a,b,sizeof(V3));
  71. }
  72.  
  73.  
  74. void gl_MulM4V3(V3 *a,M4 *b,V3 *c)
  75. {
  76.      a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3];
  77.      a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3];
  78.      a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3];
  79. }
  80.  
  81. void gl_MulM3V3(V3 *a,M4 *b,V3 *c)
  82. {
  83.      a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z;
  84.      a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z;
  85.      a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z;
  86. }
  87.  
  88. void gl_M4_MulV4(V4 *a,M4 *b,V4 *c)
  89. {
  90.      a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3]*c->W;
  91.      a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3]*c->W;
  92.      a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3]*c->W;
  93.      a->W=b->m[3][0]*c->X+b->m[3][1]*c->Y+b->m[3][2]*c->Z+b->m[3][3]*c->W;
  94. }
  95.     
  96. /* transposition of a 4x4 matrix */
  97. void gl_M4_Transpose(M4 *a,M4 *b)
  98. {
  99.   a->m[0][0]=b->m[0][0]; 
  100.   a->m[0][1]=b->m[1][0]; 
  101.   a->m[0][2]=b->m[2][0]; 
  102.   a->m[0][3]=b->m[3][0]; 
  103.  
  104.   a->m[1][0]=b->m[0][1]; 
  105.   a->m[1][1]=b->m[1][1]; 
  106.   a->m[1][2]=b->m[2][1]; 
  107.   a->m[1][3]=b->m[3][1]; 
  108.  
  109.   a->m[2][0]=b->m[0][2]; 
  110.   a->m[2][1]=b->m[1][2]; 
  111.   a->m[2][2]=b->m[2][2]; 
  112.   a->m[2][3]=b->m[3][2]; 
  113.  
  114.   a->m[3][0]=b->m[0][3]; 
  115.   a->m[3][1]=b->m[1][3]; 
  116.   a->m[3][2]=b->m[2][3]; 
  117.   a->m[3][3]=b->m[3][3]; 
  118. }
  119.  
  120. /* inversion of an orthogonal matrix of type Y=M.X+P */ 
  121. void gl_M4_InvOrtho(M4 *a,M4 b)
  122. {
  123.     int i,j;
  124.     float s;
  125.     for(i=0;i<3;i++)
  126.     for(j=0;j<3;j++) a->m[i][j]=b.m[j][i];
  127.     a->m[3][0]=0.0; a->m[3][1]=0.0; a->m[3][2]=0.0; a->m[3][3]=1.0;
  128.     for(i=0;i<3;i++) {
  129.         s=0;
  130.         for(j=0;j<3;j++) s-=b.m[j][i]*b.m[j][3];
  131.         a->m[i][3]=s;
  132.     }
  133. }
  134.  
  135. /* Inversion of a general nxn matrix.
  136.    Note : m is destroyed */
  137.  
  138. int Matrix_Inv(float *r,float *m,int n)
  139. {
  140.      int i,j,k,l;
  141.      float max,tmp,t;
  142.  
  143.      /* identitée dans r */
  144.      for(i=0;i<n*n;i++) r[i]=0;
  145.      for(i=0;i<n;i++) r[i*n+i]=1;
  146.      
  147.      for(j=0;j<n;j++) {
  148.             
  149.             /* recherche du nombre de plus grand module sur la colonne j */
  150.             max=m[j*n+j];
  151.             k=j;
  152.             for(i=j+1;i<n;i++)
  153.                 if (fabs(m[i*n+j])>fabs(max)) {
  154.                      k=i;
  155.                      max=m[i*n+j];
  156.                 }
  157.  
  158.       /* non intersible matrix */
  159.       if (max==0) return 1;
  160.  
  161.             
  162.             /* permutation des lignes j et k */
  163.             if (k!=j) {
  164.                  for(i=0;i<n;i++) {
  165.                         tmp=m[j*n+i];
  166.                         m[j*n+i]=m[k*n+i];
  167.                         m[k*n+i]=tmp;
  168.                         
  169.                         tmp=r[j*n+i];
  170.                         r[j*n+i]=r[k*n+i];
  171.                         r[k*n+i]=tmp;
  172.                  }
  173.             }
  174.             
  175.             /* multiplication de la ligne j par 1/max */
  176.             max=1/max;
  177.             for(i=0;i<n;i++) {
  178.                  m[j*n+i]*=max;
  179.                  r[j*n+i]*=max;
  180.             }
  181.             
  182.             
  183.             for(l=0;l<n;l++) if (l!=j) {
  184.                  t=m[l*n+j];
  185.                  for(i=0;i<n;i++) {
  186.                         m[l*n+i]-=m[j*n+i]*t;
  187.                         r[l*n+i]-=r[j*n+i]*t;
  188.                  }
  189.             }
  190.      }
  191.  
  192.      return 0;
  193. }
  194.  
  195.  
  196. /* inversion of a 4x4 matrix */
  197.  
  198. void gl_M4_Inv(M4 *a,M4 *b)
  199. {
  200.   M4 tmp;
  201.   memcpy(&tmp, b, 16*sizeof(float));
  202.   /*tmp=*b;*/
  203.   Matrix_Inv(&a->m[0][0],&tmp.m[0][0],4);
  204. }
  205.  
  206. void gl_M4_Rotate(M4 *a,float t,int u)
  207. {
  208.      float s,c;
  209.      int v,w;
  210.    if ((v=u+1)>2) v=0;
  211.      if ((w=v+1)>2) w=0;
  212.      s=sin(t);
  213.      c=cos(t);
  214.      gl_M4_Id(a);
  215.      a->m[v][v]=c;    a->m[v][w]=-s;
  216.      a->m[w][v]=s;    a->m[w][w]=c;
  217. }
  218.     
  219.  
  220. /* inverse of a 3x3 matrix */
  221. void gl_M3_Inv(M3 *a,M3 *m)
  222. {
  223.      float det;
  224.      
  225.      det = m->m[0][0]*m->m[1][1]*m->m[2][2]-m->m[0][0]*m->m[1][2]*m->m[2][1]-
  226.          m->m[1][0]*m->m[0][1]*m->m[2][2]+m->m[1][0]*m->m[0][2]*m->m[2][1]+
  227.          m->m[2][0]*m->m[0][1]*m->m[1][2]-m->m[2][0]*m->m[0][2]*m->m[1][1];
  228.  
  229.      a->m[0][0] = (m->m[1][1]*m->m[2][2]-m->m[1][2]*m->m[2][1])/det;
  230.      a->m[0][1] = -(m->m[0][1]*m->m[2][2]-m->m[0][2]*m->m[2][1])/det;
  231.      a->m[0][2] = -(-m->m[0][1]*m->m[1][2]+m->m[0][2]*m->m[1][1])/det;
  232.      
  233.      a->m[1][0] = -(m->m[1][0]*m->m[2][2]-m->m[1][2]*m->m[2][0])/det;
  234.      a->m[1][1] = (m->m[0][0]*m->m[2][2]-m->m[0][2]*m->m[2][0])/det;
  235.      a->m[1][2] = -(m->m[0][0]*m->m[1][2]-m->m[0][2]*m->m[1][0])/det;
  236.  
  237.      a->m[2][0] = (m->m[1][0]*m->m[2][1]-m->m[1][1]*m->m[2][0])/det;
  238.      a->m[2][1] = -(m->m[0][0]*m->m[2][1]-m->m[0][1]*m->m[2][0])/det;
  239.      a->m[2][2] = (m->m[0][0]*m->m[1][1]-m->m[0][1]*m->m[1][0])/det;
  240. }
  241.  
  242.                                                                                                         
  243. /* vector arithmetic */
  244.  
  245. int gl_V3_Norm(V3 *a)
  246. {
  247.     float n;
  248.     n=sqrt(a->X*a->X+a->Y*a->Y+a->Z*a->Z);
  249.     if (n==0) return 1;
  250.     a->X/=n;
  251.     a->Y/=n;
  252.     a->Z/=n;
  253.     return 0;
  254. }
  255.  
  256. V3 gl_V3_New(float x,float y,float z)
  257. {
  258.      V3 a;
  259.      a.X=x;
  260.      a.Y=y;
  261.      a.Z=z;
  262.      return a;
  263. }
  264.  
  265. V4 gl_V4_New(float x,float y,float z,float w)
  266. {
  267.   V4 a;
  268.   a.X=x;
  269.   a.Y=y;
  270.   a.Z=z;
  271.   a.W=w;
  272.   return a;
  273. }
  274.  
  275.  
  276.